//
// GPSMapEdit
// (c) Konstantin Galichsky (kg@geopainting.com), 2007
//
// Fix right angles for polygon.
//

# include "StdAfx.h"
# include "RectifyPolygon.h"
# include "Map.h"
# include "SetElementPoints.h"
# include "fast_floor.h"
# include "Globals.h"

const int c_nThresholdAngle  = 10;
const int c_nThresholdAngle2 = 20;

static
double _NormalizeAngle360 (double _a) {
	if (_a >= 360)
		_a -= 360;
	else if (_a < 0)
		_a += 360;
	assert (0 <= _a && _a < 360);
	return _a;
}

static
double _NormalizeAngle180 (double _a) {
	if (_a >= 180)
		_a -= 360;
	else if (_a < -180)
		_a += 360;
	assert (-180 <= _a && _a < 180);
	return _a;
}

static
bool _RectifyElement (points_t & _points) {
	const size_t cPoints = _points.size ();
	if (cPoints < 4)
		return false;

	ATLTRACE ("BEGIN - _RectifyElement()\n");

	const float fCosY = ::cos (_points [0].y*c_PI/180);
	
	// Calculate angles and sides' lengths.
	double * const pfAngleOfSide  = (double *) alloca (sizeof (double)*cPoints);
	double * const pfLengthOfSide = (double *) alloca (sizeof (double)*cPoints);
	size_t cSide;
	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const point_t & p0 = _points [cSide];
		const point_t & p1 = _points [cSide + 1 < cPoints ? cSide + 1 : 0];

		pfLengthOfSide [cSide] = ::hypot ((p1.x - p0.x)*fCosY, p1.y - p0.y);

		const double fA = _NormalizeAngle360 (::atan2 (p1.y - p0.y, (p1.x - p0.x)*fCosY)*180/c_PI);
		pfAngleOfSide [cSide] = fA;
	}

	// Check which nodes are about rectangular.
	bool * const pbNodeOK = (bool *) alloca (sizeof (bool)*cPoints);
	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const double fA_this = pfAngleOfSide [cSide];
		const double fA_prev = pfAngleOfSide [cSide >= 1 ? cSide - 1 : cPoints - 1];
		const double fDA = _NormalizeAngle360 (fA_this - fA_prev);

		const bool bAboutRect = ::fabs (fDA - 90) < c_nThresholdAngle2 || ::fabs (fDA - 270) < c_nThresholdAngle2;
		pbNodeOK [cSide] = bAboutRect;

		ATLTRACE ("side=%d\tlen=%.1f\ta=%.1f\tda=%.1f\tabout_rect=%d\n", cSide, 1e5*pfLengthOfSide [cSide], pfAngleOfSide [cSide], fDA > 180 ? 360 - fDA : fDA, pbNodeOK [cSide]);
	}

	// Count rough statistics by angles.
	size_t sides [90];
	::memset (sides, 0, sizeof (sides));
	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const bool bNodeOK_this = pbNodeOK [cSide];
		const bool bNodeOK_next = pbNodeOK [cSide + 1 < cPoints ? cSide + 1 : 0];
		if (! bNodeOK_this && ! bNodeOK_next)
			continue;

		int nA = fast_floor (pfAngleOfSide [cSide] + .5f);
		if (nA == 360)
			nA = 0;
		assert (0 <= nA && nA <= 359);
		// Ignore direction.
		const int nA2 = nA < 180 ? nA : nA - 180;
		// Ignore delta in 90 degrees.
		const int nA3 = nA2 < 90 ? nA2 : nA2 - 90;
		assert (0 <= nA3 && nA3 < 90);

		++ sides [nA3];
	}

	// Smooth the statistics function.
	size_t sides_smooth [90];
	::memset (sides_smooth, 0, sizeof (sides_smooth));
	int nAngle;
	for (nAngle = 0; nAngle < 90; ++ nAngle) {
		for (int n = -c_nThresholdAngle; n <= c_nThresholdAngle; ++ n) {
			int nn = nAngle + n;
			if (nn < 0)
				nn += 90;
			else if (nn >= 90)
				nn -= 90;
			assert (0 <= nn && nn < 90);

			sides_smooth [nAngle] += sides [nn];
		}
	}

	// Find the most frequent angle.
	int nMostFrequentAngle = 0;
	size_t cMax = 0;
	for (nAngle = 0; nAngle < 90; ++ nAngle) {
		if (cMax < sides_smooth [nAngle]) {
			cMax = sides_smooth [nAngle];
			nMostFrequentAngle = nAngle;
		}
	}

	// Determine the average direction angle.
	double fAveDirAngle = 0;
	double fLenOfSidesToRectify = 0;
	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const bool bNodeOK_this = pbNodeOK [cSide];
		const bool bNodeOK_next = pbNodeOK [cSide + 1 < cPoints ? cSide + 1 : 0];
		if (! bNodeOK_this && ! bNodeOK_next)
			continue;

		const double fAngleOfSide = pfAngleOfSide [cSide];
		// NOTE: size length is used as the 'weight' for averaging.
		const double l = pfLengthOfSide [cSide];

		const double da = _NormalizeAngle180 (fAngleOfSide - nMostFrequentAngle);
		if (::fabs (da) < c_nThresholdAngle + 1) {
			fAveDirAngle += (da + nMostFrequentAngle)*l;
			fLenOfSidesToRectify += l;
		} else if (::fabs (da - 90) < c_nThresholdAngle + 1) {
			fAveDirAngle += (da + nMostFrequentAngle - 90)*l;
			fLenOfSidesToRectify += l;
		} else if (::fabs (da + 90) < c_nThresholdAngle + 1) {
			fAveDirAngle += (da + nMostFrequentAngle + 90)*l;
			fLenOfSidesToRectify += l;
		} else if (::fabs (da - 180) < c_nThresholdAngle + 1) {
			fAveDirAngle += (da + nMostFrequentAngle - 180)*l;
			fLenOfSidesToRectify += l;
		} else if (::fabs (da + 180) < c_nThresholdAngle + 1) {
			fAveDirAngle += (da + nMostFrequentAngle + 180)*l;
			fLenOfSidesToRectify += l;
		}
	}
	if (fLenOfSidesToRectify == 0)
		return false;
	fAveDirAngle /= fLenOfSidesToRectify;
	ATLTRACE ("Ave dir angle=%.1f %.1f %.1f %.1f\n", fAveDirAngle, fAveDirAngle + 90, fAveDirAngle + 180, fAveDirAngle + 270);

	// Fix sides' angles to the average direction.
	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const bool bNodeOK_this = pbNodeOK [cSide];
		const bool bNodeOK_next = pbNodeOK [cSide + 1 < cPoints ? cSide + 1 : 0];
		if (! bNodeOK_this && ! bNodeOK_next)
			continue;

		double fAngleOfSide = pfAngleOfSide [cSide];

		const double da = _NormalizeAngle360 (fAngleOfSide - nMostFrequentAngle);
		if (::fabs (da) < c_nThresholdAngle + 1)
			fAngleOfSide = fAveDirAngle;
		else if (::fabs (da -  90) < c_nThresholdAngle + 1)
			fAngleOfSide = fAveDirAngle + 90;
		else if (::fabs (da - 180) < c_nThresholdAngle + 1)
			fAngleOfSide = fAveDirAngle + 180;
		else if (::fabs (da - 270) < c_nThresholdAngle + 1)
			fAngleOfSide = fAveDirAngle + 270;
		else if (::fabs (da - 360) < c_nThresholdAngle + 1)
			fAngleOfSide = fAveDirAngle + 360;
		else
			continue;

		pfAngleOfSide [cSide] = _NormalizeAngle360 (fAngleOfSide);
		ATLTRACE ("side=%d new_angle=%.1f\n", cSide, pfAngleOfSide [cSide]);
	}

	// Fix the nodes.
	points_t pointsNew (cPoints);
	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const size_t cSide1 = cSide + 1 < cPoints ? cSide + 1 : cSide + 1 - cPoints;
		const size_t cSide2 = cSide + 2 < cPoints ? cSide + 2 : cSide + 2 - cPoints;

		const point_t & p0 = _points [cSide];
		const point_t & p1 = _points [cSide1];
		const point_t & p2 = _points [cSide2];

		// NOTE: the center point of each side should not change.
		const double cx0 = (double (p0.x) + p1.x)/2;
		const double cy0 = (double (p0.y) + p1.y)/2;
		const double cx1 = (double (p1.x) + p2.x)/2;
		const double cy1 = (double (p1.y) + p2.y)/2;

		const double fA0 = pfAngleOfSide [cSide];
		const double fA1 = pfAngleOfSide [cSide1];
		const double fSinA0 = ::sin (fA0*c_PI/180);
		const double fCosA0 = ::cos (fA0*c_PI/180);
		const double fSinA1 = ::sin (fA1*c_PI/180);
		const double fCosA1 = ::cos (fA1*c_PI/180);
		const double fN = fCosA1*fSinA0 - fCosA0*fSinA1;
		if (::fabs (fN) < 1e-8) {
			pointsNew [cSide1] = _points [cSide1];
		} else {
			const double t = ((cy1 - cy0)*fCosA0 - (cx1 - cx0)*fCosY*fSinA0)/fN;

			pointsNew [cSide1].x = cx1 + t*fCosA1/fCosY;
			pointsNew [cSide1].y = cy1 + t*fSinA1;
		}
	}

# ifdef _DEBUG
	ATLTRACE ("--- Results ----\n");
	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const point_t & p0 = _points [cSide];
		const point_t & p1 = _points [cSide + 1 < cPoints ? cSide + 1 : 0];
		pfAngleOfSide [cSide] = _NormalizeAngle360 (::atan2 (p1.y - p0.y, (p1.x - p0.x)*fCosY)*180/c_PI);
	}

	for (cSide = 0; cSide < cPoints; ++ cSide) {
		const double fA_this = pfAngleOfSide [cSide];
		const double fA_prev = pfAngleOfSide [cSide >= 1 ? cSide - 1 : cPoints - 1];
		const double fDA = _NormalizeAngle360 (fA_this - fA_prev);

		ATLTRACE ("side=%d da=%.1f\n", cSide, fDA > 180 ? 360 - fDA : fDA);
	}
# endif // _DEBUG

	if (_points == pointsNew)
		return false;

	_points.swap (pointsNew);
	return true;
}

void RectifyPolygon (rgn_t & _rgn) {
	const size_t cElements = _rgn.elements.size ();
	for (size_t c = 0; c < cElements; ++ c) {
		points_t points = _rgn.elements [c].points; 
		if (_RectifyElement (points))
			SetElementPoints (& _rgn, c, points, point_extras_t ());
	}
}